#include <stdio.h>
#include <string.h>
#include <ctype.h>
#include <stdarg.h>
#include <time.h>

#include "socket.h"
#include "snmp.h"
#include "snmp_custom.h"

/********************************************************************************************/
/* SNMP : Functions declaration                                                             */
/********************************************************************************************/
// SNMP Parsing functions
int32_t findEntry(uint8_t *oid, int32_t len);
int32_t getOID(int32_t id, uint8_t *oid, uint8_t *len);
int32_t getValue(uint8_t *vptr, int32_t vlen);
int32_t getEntry(int32_t id, uint8_t *dataType, void *ptr, int32_t *len);
int32_t setEntry(int32_t id, void *val, int32_t vlen, uint8_t dataType, int32_t index);
int32_t makeTrapVariableBindings(dataEntryType *oid_data, void *ptr, uint32_t *len);

int32_t parseLength(const uint8_t *msg, int32_t *len);
int32_t parseTLV(const uint8_t *msg, int32_t index, tlvStructType *tlv);
void insertRespLen(int32_t reqStart, int32_t respStart, int32_t size);
int32_t parseVarBind(int32_t reqType, int32_t index);
int32_t parseSequence(int32_t reqType, int32_t index);
int32_t parseSequenceOf(int32_t reqType);
int32_t parseRequest();
int32_t parseCommunity();
int32_t parseVersion();
int32_t parseSNMPMessage();

// Debugging function
#ifdef _SNMP_DEBUG_
void dumpCode(uint8_t* header, uint8_t* tail, uint8_t *buff, int32_t len);
#endif

// Utils
void ipToByteArray(int8_t *ip, uint8_t *pDes);

/********************************************************************************************/
/* SNMP : Variable declaration                                                              */
/********************************************************************************************/
// SNMP message structures
struct messageStruct request_msg;
struct messageStruct response_msg;

// SNMP Time counter
static time_t startTime = 0;
volatile uint32_t snmp_tick_10ms = 0; //volatile uint32_t snmp_tick_1ms = 0;

// SNMP Sockets
static uint8_t SOCK_SNMP_AGENT;
static uint8_t SOCK_SNMP_TRAP;

uint8_t packet_trap[MAX_TRAPMSG_LEN] = {0,};
uint8_t errorStatus, errorIndex;


/********************************************************************************************/
/* SNMP : Time handler                                                                      */
/********************************************************************************************/
void currentUptime(void *ptr, uint8_t *len) {
    time_t curTime = getSNMPTimeTick();

    //*(uint32_t *)ptr = (uint32_t)(curTime - startTime) / 10; // calculation for 1ms tick
    *(uint32_t *)ptr = (uint32_t)(curTime - startTime); // calculation for 10ms tick
    *len = 4;
}

void SNMP_time_handler(void) {
    //snmp_tick_1ms++;
    snmp_tick_10ms++;
}

uint32_t getSNMPTimeTick(void) {
    //return snmp_tick_1ms;
    return snmp_tick_10ms;
}


/********************************************************************************************/
/* SNMP : Library Part                                                                      */
/********************************************************************************************/
/**
    @addtogroup snmp_module
    @{
*/

/**
    Initialize SNMP Daemon.
    This should be called just one time at first time

    @param none
    @return none
*/
void snmpd_init(uint8_t * managerIP, uint8_t * agentIP, uint8_t sn_agent, uint8_t sn_trap) {
#ifdef _SNMP_DEBUG_
    printf("\r\n - SNMP : Start SNMP Agent Daemon\r\n");
#endif
    SOCK_SNMP_AGENT = sn_agent;
    SOCK_SNMP_TRAP = sn_trap;

    if ((SOCK_SNMP_AGENT > _WIZCHIP_SOCK_NUM_) || (SOCK_SNMP_TRAP > _WIZCHIP_SOCK_NUM_)) {
        return;
    }

    startTime = getSNMPTimeTick(); // Start time (unit: 10ms)
    initTable(); // Settings for OID entry values

    initial_Trap(managerIP, agentIP);

    /*
        // Example Codes for SNMP Trap
        {
    		dataEntryType enterprise_oid = {0x0a, {0x2b, 0x06, 0x01, 0x04, 0x01, 0x81, 0x9b, 0x19, 0x01, 0x00},
        	    									SNMPDTYPE_OBJ_ID, 0x0a, {"\x2b\x06\x01\x04\x01\x81\x9b\x19\x10\x00"},	NULL, NULL};

    		dataEntryType trap_oid1 = {8, {0x2b, 6, 1, 4, 1, 0, 11, 0}, SNMPDTYPE_OCTET_STRING, 30, {""}, NULL, NULL};
    		dataEntryType trap_oid2 = {8, {0x2b, 6, 1, 4, 1, 0, 12, 0}, SNMPDTYPE_INTEGER, 4, {""}, NULL, NULL};

    		strcpy((char *)trap_oid1.u.octetstring, "Alert!!!"); 	// String added
    		trap_oid2.u.intval = 123456;	// Integer value added

    		// Generic Trap: warmStart
    		snmp_sendTrap((void *)"192.168.0.214", (void *)"192.168.0.112", (void *)"public", enterprise_oid, SNMPTRAP_WARMSTART, 0, 0);

    		// Enterprise-Specific Trap
    		snmp_sendTrap((void *)"192.168.0.214", (void *)"192.168.0.112", (void *)"public", enterprise_oid, 6, 0, 2, &trap_oid1, &trap_oid2);
    	}
    */
}


/**
    SNMP Process Handler.
    UDP Socket and SNMP Agent transaction handling.

    @param none
    @return none
*/

int32_t snmpd_run(void) {
    int32_t ret;
    int32_t len = 0;

    uint8_t svr_addr[6];
    uint16_t  svr_port;
#if 1
    // 20231019 taylor
    uint8_t addr_len;
#endif

    if (SOCK_SNMP_AGENT > _WIZCHIP_SOCK_NUM_) {
        return -99;
    }

    switch (getSn_SR(SOCK_SNMP_AGENT)) {
    case SOCK_UDP :
        if ((len = getSn_RX_RSR(SOCK_SNMP_AGENT)) > 0) {
#if 1
            // 20231019 taylor//teddy 240122
#if ((_WIZCHIP_ == 6100) || (_WIZCHIP_ == 6300))
            request_msg.len = recvfrom(SOCK_SNMP_AGENT, request_msg.buffer, len, svr_addr, &svr_port, &addr_len);
#else
            request_msg.len = recvfrom(SOCK_SNMP_AGENT, request_msg.buffer, len, svr_addr, &svr_port);
#endif
#else
            request_msg.len = recvfrom(SOCK_SNMP_AGENT, request_msg.buffer, len, svr_addr, &svr_port);
#endif
        } else {
            request_msg.len = 0;
        }

        if (request_msg.len > 0) {
#ifdef _SNMP_DEBUG_
            dumpCode((void *)"\r\n[Request]\r\n", (void *)"\r\n", request_msg.buffer, request_msg.len);
#endif
            // Initialize
            request_msg.index = 0;
            response_msg.index = 0;
            errorStatus = errorIndex = 0;
            memset(response_msg.buffer, 0x00, MAX_SNMPMSG_LEN);

            // Received message parsing and send response process
            if (parseSNMPMessage() != -1) {
#if 1
                // 20231016 taylor//teddy 240122
#if ((_WIZCHIP_ == 6100) || (_WIZCHIP_ == 6300))
                sendto(SOCK_SNMP_AGENT, response_msg.buffer, response_msg.index, svr_addr, svr_port, 4);
#else
                sendto(SOCK_SNMP_AGENT, response_msg.buffer, response_msg.index, svr_addr, svr_port);
#endif
#else
                sendto(SOCK_SNMP_AGENT, response_msg.buffer, response_msg.index, svr_addr, svr_port);
#endif
            }

#ifdef _SNMP_DEBUG_
            dumpCode((void *)"\r\n[Response]\r\n", (void *)"\r\n", response_msg.buffer, response_msg.index);
#endif
        }
        break;

    case SOCK_CLOSED :
        if ((ret = socket(SOCK_SNMP_AGENT, Sn_MR_UDP, PORT_SNMP_AGENT, 0x00)) != SOCK_SNMP_AGENT) {
            return ret;
        }
#ifdef _SNMP_DEBUG_
        printf(" - [%d] UDP Socket for SNMP Agent, port [%d]\r\n", SOCK_SNMP_AGENT, PORT_SNMP_AGENT);
#endif
        break;

    default :
        break;
    }


    return 1;
}


int32_t findEntry(uint8_t *oid, int32_t len) {
    int32_t i;

    for (i = 0 ; i < maxData ; i++) {
        if (len == snmpData[i].oidlen) {
            if (!memcmp(snmpData[i].oid, oid, len)) {
                return (i);
            }
        }
    }

    return OID_NOT_FOUND;
}


int32_t getOID(int32_t id, uint8_t *oid, uint8_t *len) {
    int32_t j;

    if (!((id >= 0) && (id < maxData))) {
        return INVALID_ENTRY_ID;
    }

    *len = snmpData[id].oidlen;

    for (j = 0 ; j < *len ; j++) {
        oid[j] = snmpData[id].oid[j];
    }

    return SNMP_SUCCESS;
}


int32_t getValue(uint8_t *vptr, int32_t vlen) {
    int32_t index = 0;
    int32_t value = 0;

    while (index < vlen) {
        if (index != 0) {
            value <<= 8;
        }
        value |= vptr[index++];
    }

    return value;
}


int32_t getEntry(int32_t id, uint8_t *dataType, void *ptr, int32_t *len) {
    uint8_t * ptr_8;
    int32_t value;

    uint8_t * string;
    int32_t j;

    if (!((id >= 0) && (id < maxData))) {
        return INVALID_ENTRY_ID;
    }

    *dataType = snmpData[id].dataType;

    switch (*dataType) {
    case SNMPDTYPE_OCTET_STRING :
    case SNMPDTYPE_OBJ_ID : {
        string = ptr;

        if (snmpData[id].getfunction != NULL) {
            snmpData[id].getfunction((void *)&snmpData[id].u.octetstring, &snmpData[id].dataLen);
        }

        if ((*dataType) == SNMPDTYPE_OCTET_STRING) {
            snmpData[id].dataLen = (uint8_t)strlen((char const*)&snmpData[id].u.octetstring);
        }

        *len = snmpData[id].dataLen;
        for (j = 0 ; j < *len ; j++) {
            string[j] = snmpData[id].u.octetstring[j];
        }
    }
    break;

    case SNMPDTYPE_INTEGER :
    case SNMPDTYPE_TIME_TICKS :
    case SNMPDTYPE_COUNTER :
    case SNMPDTYPE_GAUGE : {
        if (snmpData[id].getfunction != NULL) {
            snmpData[id].getfunction((void *)&snmpData[id].u.intval, &snmpData[id].dataLen);
        }

        if (snmpData[id].dataLen) {
            *len = snmpData[id].dataLen;
        } else {
            *len = sizeof(uint32_t);
        }

        /*
            // Original code (IAR, STM32)
            // This code is not working in NXP+LPCXpresso (32-bit pointer operation error)
            value = (int32_t *)ptr;
            value = HTONL(snmpData[id].u.intval);
        */

        ptr_8 = ptr;
        //value = HTONL(snmpData[id].u.intval); // Endian convert when processing 32bit pointer operation
        value = snmpData[id].u.intval;

        for (j = 0 ; j < *len ; j++) {
            ptr_8[j] = (uint8_t)((value >> ((*len - j - 1) * 8)));
        }
    }
    break;

    default :
        return INVALID_DATA_TYPE;
    }

    return SNMP_SUCCESS;
}


int32_t setEntry(int32_t id, void *val, int32_t vlen, uint8_t dataType, int32_t index) {

    int32_t retStatus = OID_NOT_FOUND;
    int32_t j;

    if (snmpData[id].dataType != dataType) {
        errorStatus = BAD_VALUE;
        errorIndex = index;
        return INVALID_DATA_TYPE;
    }

    switch (snmpData[id].dataType) {
    case SNMPDTYPE_OCTET_STRING :
    case SNMPDTYPE_OBJ_ID : {
        uint8_t *string = val;
        for (j = 0 ; j < vlen ; j++) {
            snmpData[id].u.octetstring[j] = string[j];
        }
        snmpData[id].dataLen = vlen;
    }
    retStatus = SNMP_SUCCESS;
    break;

    case SNMPDTYPE_INTEGER :
    case SNMPDTYPE_TIME_TICKS :
    case SNMPDTYPE_COUNTER :
    case SNMPDTYPE_GAUGE : {
        snmpData[id].u.intval = getValue((uint8_t *)val, vlen);
        snmpData[id].dataLen = vlen;

        if (snmpData[id].setfunction != NULL) {
            snmpData[id].setfunction(snmpData[id].u.intval);
        }

    }
    retStatus = SNMP_SUCCESS;
    break;

    default :
        retStatus = INVALID_DATA_TYPE;
        break;

    }

    return retStatus;
}


int32_t parseLength(const uint8_t *msg, int32_t *len) {
    int32_t i = 1;

    if (msg[0] & 0x80) {
        int32_t tlen = (msg[0] & 0x7f) - 1;
        *len = msg[i++];

        while (tlen--) {
            *len <<= 8;
            *len |= msg[i++];
        }
    } else {
        *len = msg[0];
    }

    return i;
}


int32_t parseTLV(const uint8_t *msg, int32_t index, tlvStructType *tlv) {
    int32_t Llen = 0;

    tlv->start = index;

    Llen = parseLength((const uint8_t *)&msg[index + 1], &tlv->len);

    tlv->vstart = index + Llen + 1;

    switch (msg[index]) {
    case SNMPDTYPE_SEQUENCE:
    case GET_REQUEST:
    case GET_NEXT_REQUEST:
    case SET_REQUEST:
        tlv->nstart = tlv->vstart;
        break;
    default:
        tlv->nstart = tlv->vstart + tlv->len;
        break;
    }

    return 0;
}


void insertRespLen(int32_t reqStart, int32_t respStart, int32_t size) {
    int32_t indexStart, lenLength;
    uint32_t mask = 0xff;
    int32_t shift = 0;

    if (request_msg.buffer[reqStart + 1] & 0x80) {
        lenLength = request_msg.buffer[reqStart + 1] & 0x7f;
        indexStart = respStart + 2;

        while (lenLength--) {
            response_msg.buffer[indexStart + lenLength] =
                (uint8_t)((size & mask) >> shift);
            shift += 8;
            mask <<= shift;
        }
    } else {
        response_msg.buffer[respStart + 1] = (uint8_t)(size & 0xff);
    }
}

int32_t parseVarBind(int32_t reqType, int32_t index) {
    int32_t seglen = 0, id;
    tlvStructType name, value;
    int32_t size = 0;

    //extern const int32_t maxData;

    parseTLV(request_msg.buffer, request_msg.index, &name);

    if (request_msg.buffer[name.start] != SNMPDTYPE_OBJ_ID) {
        return -1;
    }

    id = findEntry(&request_msg.buffer[name.vstart], name.len);

    if ((reqType == GET_REQUEST) || (reqType == SET_REQUEST)) {
        seglen = name.nstart - name.start;
        COPY_SEGMENT(name);
        size = seglen;
    } else if (reqType == GET_NEXT_REQUEST) {
        response_msg.buffer[response_msg.index] = request_msg.buffer[name.start];

        if (++id >= maxData) {
            id = OID_NOT_FOUND;
            seglen = name.nstart - name.start;
            COPY_SEGMENT(name);
            size = seglen;
        } else {
            request_msg.index += name.nstart - name.start;

            getOID(id, &response_msg.buffer[response_msg.index + 2], &response_msg.buffer[response_msg.index + 1]);

            seglen = response_msg.buffer[response_msg.index + 1] + 2;
            response_msg.index += seglen ;
            size = seglen;
        }
    }

    parseTLV(request_msg.buffer, request_msg.index, &value);

    if (id != OID_NOT_FOUND) {
        uint8_t dataType;
        int32_t len;

        if ((reqType == GET_REQUEST) || (reqType == GET_NEXT_REQUEST)) {
            getEntry(id, &dataType, &response_msg.buffer[response_msg.index + 2], &len);

            response_msg.buffer[response_msg.index] = dataType;
            response_msg.buffer[response_msg.index + 1] = len;
            seglen = (2 + len);
            response_msg.index += seglen;

            request_msg.index += (value.nstart - value.start);

        } else if (reqType == SET_REQUEST) {
            setEntry(id, &request_msg.buffer[value.vstart], value.len, request_msg.buffer[value.start], index);
            seglen = value.nstart - value.start;
            COPY_SEGMENT(value);
        }
    } else {
        seglen = value.nstart - value.start;
        COPY_SEGMENT(value);

        errorIndex = index;
        errorStatus = NO_SUCH_NAME;
    }

    size += seglen;

    return size;
}


int32_t parseSequence(int32_t reqType, int32_t index) {
    int32_t seglen;
    tlvStructType seq;
    int32_t size = 0, respLoc;

    parseTLV(request_msg.buffer, request_msg.index, &seq);

    if (request_msg.buffer[seq.start] != SNMPDTYPE_SEQUENCE) {
        return -1;
    }

    seglen = seq.vstart - seq.start;
    respLoc = response_msg.index;
    COPY_SEGMENT(seq);

    size = parseVarBind(reqType, index);
    insertRespLen(seq.start, respLoc, size);
    size += seglen;

    return size;
}


int32_t parseSequenceOf(int32_t reqType) {
    int32_t seglen;
    tlvStructType seqof;
    int32_t size = 0, respLoc;
    int32_t index = 0;

    parseTLV(request_msg.buffer, request_msg.index, &seqof);

    if (request_msg.buffer[seqof.start] != SNMPDTYPE_SEQUENCE_OF) {
        return -1;
    }

    seglen = seqof.vstart - seqof.start;
    respLoc = response_msg.index;
    COPY_SEGMENT(seqof);

    while (request_msg.index < request_msg.len) {
        size += parseSequence(reqType, index++);
    }

    insertRespLen(seqof.start, respLoc, size);

    return size;
}


int32_t parseRequest() {
    int32_t ret, seglen;
    tlvStructType snmpreq, requestid, errStatus, errIndex;
    int32_t size = 0, respLoc, reqType;

    parseTLV(request_msg.buffer, request_msg.index, &snmpreq);

    reqType = request_msg.buffer[snmpreq.start];

    if (!VALID_REQUEST(reqType)) {
        return -1;
    }

    seglen = snmpreq.vstart - snmpreq.start;
    respLoc = snmpreq.start;
    size += seglen;
    COPY_SEGMENT(snmpreq);

    response_msg.buffer[snmpreq.start] = GET_RESPONSE;

    parseTLV(request_msg.buffer, request_msg.index, &requestid);
    seglen = requestid.nstart - requestid.start;
    size += seglen;
    COPY_SEGMENT(requestid);

    parseTLV(request_msg.buffer, request_msg.index, &errStatus);
    seglen = errStatus.nstart - errStatus.start;
    size += seglen;
    COPY_SEGMENT(errStatus);

    parseTLV(request_msg.buffer, request_msg.index, &errIndex);
    seglen = errIndex.nstart - errIndex.start;
    size += seglen;
    COPY_SEGMENT(errIndex);

    ret = parseSequenceOf(reqType);
    if (ret == -1) {
        return -1;
    } else {
        size += ret;
    }

    insertRespLen(snmpreq.start, respLoc, size);

    if (errorStatus) {
        response_msg.buffer[errStatus.vstart] = errorStatus;
        response_msg.buffer[errIndex.vstart] = errorIndex + 1;
    }

    return size;
}


int32_t parseCommunity() {
    int32_t seglen;
    tlvStructType community;
    int32_t size = 0;

    parseTLV(request_msg.buffer, request_msg.index, &community);

    if (!((request_msg.buffer[community.start] == SNMPDTYPE_OCTET_STRING) && (community.len == COMMUNITY_SIZE))) {
        return -1;
    }

    if (!memcmp(&request_msg.buffer[community.vstart], (int8_t *)COMMUNITY, COMMUNITY_SIZE)) {
        seglen = community.nstart - community.start;
        size += seglen;
        COPY_SEGMENT(community);

        size += parseRequest();
    } else {
        return -1;
    }

    return size;
}


int32_t parseVersion() {
    int32_t size = 0, seglen;
    tlvStructType tlv;

    size = parseTLV(request_msg.buffer, request_msg.index, &tlv);

    if (!((request_msg.buffer[tlv.start] == SNMPDTYPE_INTEGER) && (request_msg.buffer[tlv.vstart] == SNMP_V1))) {
        return -1;
    }

    seglen = tlv.nstart - tlv.start;
    size += seglen;
    COPY_SEGMENT(tlv);
    size = parseCommunity();

    if (size == -1) {
        return size;
    } else {
        return (size + seglen);
    }
}


int32_t parseSNMPMessage() {
    int32_t size = 0, seglen, respLoc;
    tlvStructType tlv;

    parseTLV(request_msg.buffer, request_msg.index, &tlv);

    if (request_msg.buffer[tlv.start] != SNMPDTYPE_SEQUENCE_OF) {
        return -1;
    }

    seglen = tlv.vstart - tlv.start;
    respLoc = tlv.start;
    COPY_SEGMENT(tlv);

    size = parseVersion();

    if (size == -1) {
        return -1;
    } else {
        size += seglen;
    }

    insertRespLen(tlv.start, respLoc, size);

    return 0;
}

void ipToByteArray(int8_t *ip, uint8_t *pDes) {
    uint32_t i, ip1 = 0, ip2 = 0, ip3 = 0, ip4 = 0;
    int8_t buff[32];
    uint32_t len = (uint32_t)strlen((char const*)ip);
    strcpy((char *)buff, (char const*)ip);

    for (i = 0; i < len; i++) {
        if (buff[i] == '.') {
            buff[i] = ' ';
        }
    }

    sscanf((char const*)buff, "%u %u %u %u", &ip1, &ip2, &ip3, &ip4);
    pDes[0] = ip1; pDes[1] = ip2; pDes[2] = ip3; pDes[3] = ip4;
}


int32_t makeTrapVariableBindings(dataEntryType *oid_data, void *ptr, uint32_t *len) {
    uint32_t j;

    ((uint8_t*)ptr)[0] = 0x30;
    ((uint8_t*)ptr)[1] = 0xff;
    ((uint8_t*)ptr)[2] = 0x06;
    ((uint8_t*)ptr)[3] = oid_data->oidlen;

    for (j = 0 ; j < oid_data->oidlen ; j++) {
        ((uint8_t*)ptr)[j + 4] = oid_data->oid[j];
    }

    switch (oid_data->dataType) {
    case SNMPDTYPE_OCTET_STRING :
    case SNMPDTYPE_OBJ_ID : {
        uint8_t *string = &((uint8_t*)ptr)[4 + oid_data->oidlen + 2];

        if (oid_data->dataType == SNMPDTYPE_OCTET_STRING) {
            oid_data->dataLen = (uint8_t)strlen((char const*)&oid_data->u.octetstring);
        }
        for (j = 0 ; j < oid_data->dataLen ; j++) {
            string[j] = oid_data->u.octetstring[j];
        }

        ((uint8_t*)ptr)[4 + oid_data->oidlen] = oid_data->dataType;
        ((uint8_t*)ptr)[4 + oid_data->oidlen + 1] = oid_data->dataLen;
        ((uint8_t*)ptr)[1] = 2 + oid_data->oidlen + 2 + oid_data->dataLen;
        *len = 4 + oid_data->oidlen + 2 + oid_data->dataLen;
    }
    break;

    case SNMPDTYPE_INTEGER :
    case SNMPDTYPE_TIME_TICKS :
    case SNMPDTYPE_COUNTER :
    case SNMPDTYPE_GAUGE : {
        oid_data->dataLen = 4;

        *(int32_t*)(&((uint8_t*)ptr)[4 + oid_data->oidlen + 2]) = HTONL(oid_data->u.intval);

        ((uint8_t*)ptr)[4 + oid_data->oidlen] = oid_data->dataType;
        ((uint8_t*)ptr)[4 + oid_data->oidlen + 1] = oid_data->dataLen;
        ((uint8_t*)ptr)[1] = 2 + oid_data->oidlen + 2 + oid_data->dataLen;
        *len = 4 + oid_data->oidlen + 2 + oid_data->dataLen;
    }
    break;

    default :
        return INVALID_DATA_TYPE;
    }

    return SNMP_SUCCESS;
}


int32_t snmp_sendTrap(uint8_t * managerIP, uint8_t * agentIP, int8_t* community, dataEntryType enterprise_oid, uint32_t genericTrap, uint32_t specificTrap, uint32_t va_count, ...) {
    uint32_t i;
    int32_t packet_index = 0;
    int32_t packet_buff1 = 0;
    int32_t packet_buff2 = 0;
    int32_t packet_buff3 = 0;

    va_list ap;
    uint32_t length_var_bindings = 0;
    uint32_t length_buff = 0;

    //SNMP Trap packet generation
    packet_trap[packet_index++] = 0x30; // ASN.1 Header

    packet_trap[packet_index] = 0xff; // pdu_length, temp
    packet_buff1 = packet_index++;

    packet_trap[packet_index++] = 0x02; // Version
    packet_trap[packet_index++] = 0x01;
    packet_trap[packet_index++] = 0x00;

    packet_trap[packet_index++] = 0x04; // Community
    packet_trap[packet_index++] = (uint8_t)strlen((char const*)community);
    memcpy(&(packet_trap[packet_index]), community, strlen((char const*)community));

    packet_index = packet_index + (uint8_t)strlen((char const*)community);

    packet_trap[packet_index++] = 0xa4; // trap
    packet_trap[packet_index] = 0xff; // length, temp
    packet_buff2 = packet_index++;

    packet_trap[packet_index++] = 0x06; // enterprise_oid
    packet_trap[packet_index++] = enterprise_oid.oidlen;
    for (i = 0; i < enterprise_oid.oidlen; i++) {
        packet_trap[packet_index++] = enterprise_oid.oid[i];
    }

    packet_trap[packet_index++] = 0x40; // agent ip
    packet_trap[packet_index++] = 0x04;
    packet_trap[packet_index++] = agentIP[0];
    packet_trap[packet_index++] = agentIP[1];
    packet_trap[packet_index++] = agentIP[2];
    packet_trap[packet_index++] = agentIP[3];

    packet_trap[packet_index++] = 0x02; // Generic Trap
    packet_trap[packet_index++] = 0x01;
    packet_trap[packet_index++] = (uint8_t)genericTrap;

    packet_trap[packet_index++] = 0x02; // Specific Trap
    packet_trap[packet_index++] = 0x01;
    packet_trap[packet_index++] = (uint8_t)specificTrap;

    packet_trap[packet_index++] = 0x43; // Timestamp
    packet_trap[packet_index++] = 0x01;
    packet_trap[packet_index++] = 0x00;

    packet_trap[packet_index++] = 0x30; // Sequence of variable-bindings
    packet_trap[packet_index] = 0xff;
    packet_buff3 = packet_index++;

    // variable-bindings
    {
        va_start(ap, va_count);

        for (i = 0; i < va_count; i++) {
            dataEntryType* fff = va_arg(ap, dataEntryType*);
            makeTrapVariableBindings(fff, &(packet_trap[packet_index]), &length_buff);
            packet_index = packet_index + length_buff;
            length_var_bindings = length_var_bindings + length_buff;
        }

        packet_trap[packet_buff3] = length_var_bindings;

        va_end(ap);
    }


    packet_trap[packet_buff1] = packet_index - 2;
    packet_trap[packet_buff2] = packet_index - (9 + (uint8_t)strlen((char const*)community));

    // Send SNMP Trap Packet to NMS
    {
        socket(SOCK_SNMP_TRAP, Sn_MR_UDP, PORT_SNMP_TRAP, 0);
#if 1
        // 20231016 taylor//teddy 240122
#if ((_WIZCHIP_ == 6100) || (_WIZCHIP_ == 6300))
        sendto(SOCK_SNMP_TRAP, packet_trap, packet_index, managerIP, PORT_SNMP_TRAP, 4);
#else
        sendto(SOCK_SNMP_TRAP, packet_trap, packet_index, managerIP, PORT_SNMP_TRAP);
#endif
#else
        sendto(SOCK_SNMP_TRAP, packet_trap, packet_index, managerIP, PORT_SNMP_TRAP);
#endif

        close(SOCK_SNMP_TRAP);
        return 0;
    }
}

#ifdef _SNMP_DEBUG_
void dumpCode(uint8_t* header, uint8_t* tail, uint8_t *buff, int32_t len) {
    int i;

    printf((char const*)header);

    for (i = 0; i < len; i++) {
        if (i % 16 == 0)	{
            printf("0x%04x : ", i);
        }
        printf("%02x ", buff[i]);

        if (i % 16 - 15 == 0) {
            int j;
            printf("  ");
            for (j = i - 15; j <= i; j++) {
                if (isprint(buff[j]))	{
                    printf("%c", buff[j]);
                } else {
                    printf(".");
                }
            }
            printf("\r\n");
        }
    }

    if (i % 16 != 0) {
        int j;
        int spaces = (len - i + 16 - i % 16) * 3 + 2;
        for (j = 0; j < spaces; j++) {
            printf(" ");
        }
        for (j = i - i % 16; j < len; j++) {
            if (isprint(buff[j]))	{
                printf("%c", buff[j]);
            } else {
                printf(".");
            }
        }
    }
    printf((char const*)tail);
}
#endif

